 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Integrating a detector
Description: This tutorial describes how to integrate a custom detector into world_db_model.Tutorial Level:
This tutorial shows how to integrate an existing detector into world_db_model. We will integrate the cup detector in pcl_detectors.
Contents
Detector interface
In this tutorial we assume that the cup detector provides the following action interface:
--- --- geometry_msgs/PointStamped position geometry_msgs/Vector3 axis float64 radius
The detector gets no goal. The goal is just used to start the detection and to stop the detection when the detector is not needed anymore. Detection results are always sent to world_db_model over feedback messages. The cup detector gets point clouds over the topic /stereo_cylinder_detector/narrow_stereo_textured/points2.
Detector plugin
To create a detector plugin, first create a new package with the correct dependencies:
roscreate-pkg world_db_cup_detector world_db_core world_db_detector_lib pcl_detectors
The action interface
Every world_db_model plugin provides an action interface. Normally, goals contain a spatial request, i.e. a pose and a distance around that pose. In our cup detector example, the detector plugin sends back a sequence of cup detections and does not have a feedback message:
geometry_msgs/Point position float32 radius --- geometry_msgs/Point[] points geometry_msgs/Vector3[] axis float64[] radius ---
The code
The implementation of the detector plugin consists of two files, a header file with some traits, the declaration of the database class and the actual detector plugin class, and a cpp file that implements the interfaces needed by the detector plugin class and the pluginlib declaration.
We first create the file cup_detector.h in include/cup_detector and add the following contents.
1 #ifndef __WORLD_DB_CUPS_DETECTOR_H__
2 #define __WORLD_DB_CUPS_DETECTOR_H__
3
4 #include <ros/message_traits.h>
5
6 #include <world_db_detector_lib/spatially_indexed_table.h>
7 #include <world_db_detector_lib/detector.h>
8 #include <world_db_detector_lib/simple_action_detector.h>
9 #include <world_db_detector_lib/bag_data_source.h>
10 #include <pcl_detectors/DetectCupsAction.h>
11 #include <world_db_pcl_detectors/WorldDBDetectCupsAction.h>
12
13 // The feedback message does not have the common layout of stamped
14 // messages with the header at the toplevel. We need to define message
15 // traits.
16 namespace ros
17 {
18 namespace message_traits
19 {
20
21 template<>
22 struct TimeStamp<pcl_detectors::DetectCupsFeedback>
23 {
24 static ros::Time *pointer(pcl_detectors::DetectCupsFeedback &m)
25 {
26 return &m.position.header.stamp;
27 }
28 static ros::Time const *pointer(const pcl_detectors::DetectCupsFeedback &m)
29 {
30 return &m.position.header.stamp;
31 }
32 static ros::Time value(const pcl_detectors::DetectCupsFeedback &m)
33 {
34 return m.position.header.stamp;
35 }
36 };
37
38 template<>
39 struct FrameId<pcl_detectors::DetectCupsFeedback>
40 {
41 static std::string* pointer(pcl_detectors::DetectCupsFeedback &m)
42 {
43 return &m.position.header.frame_id;
44 }
45 static std::string const* pointer(const pcl_detectors::DetectCupsFeedback &m)
46 {
47 return &m.position.header.frame_id;
48 }
49 static std::string value(const pcl_detectors::DetectCupsFeedback &m)
50 {
51 return m.position.header.frame_id;
52 }
53 };
54
55 }
56 }
57
58 namespace model_database_tables
59 {
60 // Declare the database table.
61
62 DECLARE_SPATIALLY_INDEXED_TABLE(pcl_detectors::DetectCupsFeedback, DetectedCups);
63
64 // Define the spatial index via the SpatialIndex traits class.
65 template<>
66 struct SpatialIndex<pcl_detectors::DetectCupsFeedback>
67 {
68 static double x(const pcl_detectors::DetectCupsFeedback &cup)
69 {
70 return cup.position.point.x;
71 }
72 static double y(const pcl_detectors::DetectCupsFeedback &cup)
73 {
74 return cup.position.point.y;
75 }
76 static double z(const pcl_detectors::DetectCupsFeedback &cup)
77 {
78 return cup.position.point.z;
79 }
80 };
81
82 }
83
84 namespace world_db_pcl_detectors
85 {
86
87 // Assemble the detector type
88 typedef world_db_detectors::SimpleActionDetector<pcl_detectors::DetectCupsAction, world_db_detectors::BagDataSource> cup_detector_interface_type;
89 // Define the detector class
90 class CupDetector
91 : public world_db_detectors::DetectorPlugin<WorldDBDetectCupsAction, cup_detector_interface_type, model_database_tables::DetectedCups>
92 {
93 };
94
95 }
96
97 #endif
98
Now, we create the cpp file and add the following content:
1 #include <boost/foreach.hpp>
2 #include <boost/function_output_iterator.hpp>
3 #include <boost/bind.hpp>
4
5 #include <tf/tf.h>
6 #include <pcl_detectors/DetectCupsAction.h>
7 #include <world_db_core/utilities/iterators.h>
8 #include <pluginlib/class_list_macros.h>
9 #include <world_db_pcl_detectors/cup_detector.h>
10 #include <world_db_detector_lib/simple_detector_db_cache.h>
11
12 /*
13 * Define all interfaces that are needed for the generic detector to work.
14 */
15 namespace world_db_detectors
16 {
17
18 template<>
19 struct DetectorAction<world_db_pcl_detectors::WorldDBDetectCupsAction>
20 {
21 typedef world_db_pcl_detectors::WorldDBDetectCupsAction action_server_type;
22 typedef action_server_type::_action_goal_type::_goal_type goal_type;
23 typedef action_server_type::_action_feedback_type::_feedback_type feedback_type;
24 typedef action_server_type::_action_result_type::_result_type result_type;
25
26 DetectorAction(ros::NodeHandle &) {}
27
28 /**
29 * Read the spatial request from an action goal.
30 */
31 static void goalRequest(const goal_type &goal,
32 geometry_msgs::PointStamped &position,
33 double &radius)
34 {
35 position.header.frame_id = "/map";
36 position.point = goal.position;
37 radius = goal.radius;
38 }
39
40 /**
41 * create the action result from the detector's result
42 */
43 template<typename rangeT>
44 static bool result(const rangeT &result,
45 result_type &action_result)
46 {
47 BOOST_FOREACH(pcl_detectors::DetectCupsFeedback detection, result)
48 {
49 action_result.points.push_back(detection.position.point);
50 action_result.axis.push_back(detection.axis);
51 action_result.radius.push_back(detection.radius);
52 }
53 return true;
54 }
55
56 /**
57 * For now, we do not need to send feedback.
58 */
59 template<typename T>
60 static bool feedback(const T &feedback, feedback_type &action_feedback)
61 {
62 return false;
63 }
64 };
65
66 /**
67 * Define the interface for interacting with the database. Functions
68 * for mapping previous database results and data from bags to a
69 * detector request for mapping the detector result into database
70 * entries.
71 */
72 template<>
73 struct DetectorDatabase<model_database_tables::DetectedCups, world_db_pcl_detectors::cup_detector_interface_type>
74 : public SimpleDetectorDBCache<DetectorDatabase<model_database_tables::DetectedCups, world_db_pcl_detectors::cup_detector_interface_type>,
75 world_db_pcl_detectors::CupDetector>
76 {
77 DetectorDatabase(ros::NodeHandle &) {}
78
79 static const std::vector<pcl_detectors::DetectCupsFeedback> resultSequence(const pcl_detectors::DetectCupsFeedback &msg)
80 {
81 return std::vector<pcl_detectors::DetectCupsFeedback>(&msg, &msg+1);
82 }
83 };
84
85 /**
86 * Interface to transform a message between different tf frames.
87 */
88
89 template<>
90 struct TransformedMessage<pcl_detectors::DetectCupsFeedback>
91 {
92 TransformedMessage(ros::NodeHandle &) {}
93
94 template<typename ptT>
95 static void transformPoint(const ptT &src, const tf::StampedTransform tf,
96 ptT &dst)
97 {
98 btVector3 tmp = tf * btVector3(src.x, src.y, src.z);
99 dst.x = tmp.x();
100 dst.y = tmp.y();
101 dst.z = tmp.z();
102 }
103
104 static void transform(const tf_recorder_plugin::TFRecorder &tf,
105 const pcl_detectors::DetectCupsFeedback &msg, pcl_detectors::DetectCupsFeedback &transformed_msg)
106 {
107 tf::StampedTransform msg_to_map;
108
109 transformed_msg = msg;
110
111 tf.lookupTransform(tf.referenceFrame(), msg.position.header.frame_id, msg.position.header.stamp, msg_to_map);
112 transformed_msg.position.header.frame_id = tf.referenceFrame();
113 transformPoint(msg.position.point, msg_to_map, transformed_msg.position.point);
114
115 {
116 btQuaternion rotation = msg_to_map.getRotation();
117 btVector3 tmp = btVector3(msg.axis.x, msg.axis.y, msg.axis.z).rotate(
118 rotation.getAxis(), rotation.getAngle());
119 transformed_msg.axis.x = tmp.x();
120 transformed_msg.axis.y = tmp.y();
121 transformed_msg.axis.z = tmp.z();
122 }
123 }
124 };
125
126 }
127
128 PLUGINLIB_DECLARE_CLASS(world_db_pcl_detectors, CupDetector, world_db_pcl_detectors::CupDetector, world_db_model::WorldDBModelPlugin);
The code explained
cup_detector.h
In this detector, we will use the feedback message and completely store it in the database cache. Our feedback message does not have a header at its top level. Instead, we are using the pose's header field. Therefore, we need to define message traits to tell the database interface how to look up time stamp and frame id.
1 namespace ros
2 {
3 namespace message_traits
4 {
5
6 template<>
7 struct TimeStamp<pcl_detectors::DetectCupsFeedback>
8 {
9 static ros::Time *pointer(pcl_detectors::DetectCupsFeedback &m)
10 {
11 return &m.position.header.stamp;
12 }
13 static ros::Time const *pointer(const pcl_detectors::DetectCupsFeedback &m)
14 {
15 return &m.position.header.stamp;
16 }
17 static ros::Time value(const pcl_detectors::DetectCupsFeedback &m)
18 {
19 return m.position.header.stamp;
20 }
21 };
22
23 template<>
24 struct FrameId<pcl_detectors::DetectCupsFeedback>
25 {
26 static std::string* pointer(pcl_detectors::DetectCupsFeedback &m)
27 {
28 return &m.position.header.frame_id;
29 }
30 static std::string const* pointer(const pcl_detectors::DetectCupsFeedback &m)
31 {
32 return &m.position.header.frame_id;
33 }
34 static std::string value(const pcl_detectors::DetectCupsFeedback &m)
35 {
36 return m.position.header.frame_id;
37 }
38 };
39
40 }
41 }
Next, we create the database interface. Cup detections do not have special requirements on temporal order. Therefore, we can use a spatially indexed table. We create it with the following lines:
This macro creates the new class DetectedCups that contains, apart from the spatial index (x, y, z coordinates), a frame id and a time stamp, instances of type pcl_detectors::DetectCupsFeedback. It also creates a table in the database with name DetectedCups.
Note: The DECLARE_SPATIALLY_INDEXED_TABLE macro must be called in namespace model_database_tables. |
The next step is to define the traits to tell the database class from which slots it should build the spatial index. The SpatialIndex traits class must provide three (static) methods, x, y, z. The methods get the message instance as parameter and return the corresponding value.
1 // Define the spatial index via the SpatialIndex traits class.
2 template<>
3 struct SpatialIndex<pcl_detectors::DetectCupsFeedback>
4 {
5 static double x(const pcl_detectors::DetectCupsFeedback &cup)
6 {
7 return cup.position.point.x;
8 }
9 static double y(const pcl_detectors::DetectCupsFeedback &cup)
10 {
11 return cup.position.point.y;
12 }
13 static double z(const pcl_detectors::DetectCupsFeedback &cup)
14 {
15 return cup.position.point.z;
16 }
17 };
18
19 }
The next step is to actually define the detector plugin. We first make a typedef to define the detector interface. The detector gets its input from a bag and is started and stopped with the action pcl_detectors::DetectCupsAction.
typedef world_db_detectors::SimpleActionDetector<pcl_detectors::DetectCupsAction, world_db_detectors::BagDataSource> cup_detector_interface_type;
Finally, we define the detector plugin class by deriving the class world_db_detectors::DetectorPlugin. We need to fill in the action type of the detector, the interface type we defined above and the table in which to cache the results.
cup_detector.cpp
The implementation of the DetectorPlugin class requires three traits to be defined.
DetectorAction<Action>: The interface to the detector plugin action.
DetectorDatabase<DBClass, DetectorInterface>: The interface used for caching in the database.
TransformedMessage<Message>: Interface to transform the message into the reference frame that is used for indexing in the database.
First, we define the `DetectorAction trait:
1 template<>
2 struct DetectorAction<world_db_pcl_detectors::WorldDBDetectCupsAction>
3 {
4 typedef world_db_pcl_detectors::WorldDBDetectCupsAction action_server_type;
5 typedef action_server_type::_action_goal_type::_goal_type goal_type;
6 typedef action_server_type::_action_feedback_type::_feedback_type feedback_type;
7 typedef action_server_type::_action_result_type::_result_type result_type;
8
9 DetectorAction(ros::NodeHandle &) {}
The typedefs are there only for convenience since C++ tends to become very verbose. The constructor is required by the DetectorPlugin class but not used in this specific detector plugin.
Now, we define the method to read the spatial request from the goal:
Next, we assemble the result message. Please note that the result is a sequence of detector feedback messages. The sequence type is unspecified but implements the boost range concept. Therefore, we template on the range type. The output is the result of the detector plugin action:
1 template<typename rangeT>
2 static bool result(const rangeT &result,
3 result_type &action_result)
4 {
5 BOOST_FOREACH(pcl_detectors::DetectCupsFeedback detection, result)
6 {
7 action_result.points.push_back(detection.position.point);
8 action_result.axis.push_back(detection.axis);
9 action_result.radius.push_back(detection.radius);
10 }
11 return true;
12 }
Finally, we don't use feedback in this plugin:
For caching we use the SimpleDetectorDBCache class. It implements simple caching based on timestamps. It stores all detector results in the database and uses them instead of running the detector when possible. We use the class by deriving our traits class from it.
1 template<>
2 struct DetectorDatabase<model_database_tables::DetectedCups, world_db_pcl_detectors::cup_detector_interface_type>
3 : public SimpleDetectorDBCache<DetectorDatabase<model_database_tables::DetectedCups, world_db_pcl_detectors::cup_detector_interface_type>,
4 world_db_pcl_detectors::CupDetector>
5 {
6 DetectorDatabase(ros::NodeHandle &) {}
To cache results in the database, we need to provide the actual sequence of instances that should be stored in the database. In this simple detector, this is the detector feedback message itself since the detector packs only one detection into that message. That means, we need to create a vector (or a list or a similar sequence) from that single message and return it from the method resultSequence.
The last trait interface we need to define is the functionality to transform the feedback message of the detector into the right reference frame. This interface is optional, but since the cup detector gets point clouds in the narrow stereo frame, its result is also in that frame and we need to transform it into map. We use the tf_recorder_plugin for that.
First, we declare the trait struct and the constructor.
Now, we define a little convenience function to transform a single point.
Finally, we define the actual transformation function.
1 static void transform(const tf_recorder_plugin::TFRecorder &tf,
2 const pcl_detectors::DetectCupsFeedback &msg, pcl_detectors::DetectCupsFeedback &transformed_msg)
3 {
4 tf::StampedTransform msg_to_map;
5
6 transformed_msg = msg;
7
8 tf.lookupTransform(tf.referenceFrame(), msg.position.header.frame_id, msg.position.header.stamp, msg_to_map);
9 transformed_msg.position.header.frame_id = tf.referenceFrame();
10 transformPoint(msg.position.point, msg_to_map, transformed_msg.position.point);
11
12 {
13 btQuaternion rotation = msg_to_map.getRotation();
14 btVector3 tmp = btVector3(msg.axis.x, msg.axis.y, msg.axis.z).rotate(
15 rotation.getAxis(), rotation.getAngle());
16 transformed_msg.axis.x = tmp.x();
17 transformed_msg.axis.y = tmp.y();
18 transformed_msg.axis.z = tmp.z();
19 }
20 }
Registering the plugin
To use our new plugin, we need to add it to the CMakeLists.txt file and add a plugin description for pluginlib. Open the CMakeLists.txt file in your package directory and add:
rosbuild_add_library(cup_detector src/cup_detector.cpp)
Then create the file plugin.xml and add the following content:
<library path="lib/libcup_detector">
<class name="world_db_pcl_detectors/CupDetector" type="world_db_pcl_detectors::CupDetector" base_class_type="world_db_model::WorldDBModelPlugin">
<description>
Plugin that detects cylinders that might be cups in point clouds.
</description>
</class>
</library>Finally, get the manifest right. Edit it and add:
<export>
<world_db_core plugin="${prefix}/plugins.xml"/>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`"
lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
</export>